/*********************************************************************************************************************
* seekfree_stc32g_wireless_charge_opensource_software 即（STC32G 无线充电开源软件）
* 是一个基于官方 SDK 接口的开源软件
* Copyright (c) 2022 SEEKFREE 逐飞科技
*
* 本文件是 STC32G 无线充电开源软件的一部分
*
* STC32G 无线充电开源软件 是免费软件
* 您可以根据自由软件基金会发布的 GPL（GNU General Public License，即 GNU通用公共许可证）的条款
* 即 GPL 的第3版（即 GPL3.0）或（您选择的）任何后来的版本，重新发布和/或修改它
*
* 本开源库的发布是希望它能发挥作用，但并未对其作任何的保证
* 甚至没有隐含的适销性或适合特定用途的保证
* 更多细节请参见 GPL
*
* 您应该在收到本开源库的同时收到一份 GPL 的副本
* 如果没有，请参阅<https://www.gnu.org/licenses/>
*
* 额外注明：
* 本开源库使用 GPL3.0 开源许可证协议 以上许可申明为译文版本
* 许可申明英文版在 libraries/doc 文件夹下的 GPL3_permission_statement.txt 文件中
* 许可证副本在 libraries 文件夹下 即该文件夹下的 LICENSE 文件
* 欢迎各位使用并传播本程序 但修改内容时必须保留逐飞科技的版权声明（即本声明）
*
* 文件名称          charge_control
* 公司名称          成都逐飞科技有限公司
* 版本信息          查看 libraries/doc 文件夹内 version 文件 版本说明
* 开发环境          MDK 5.37
* 适用平台          STC32G
* 店铺链接          https://seekfree.taobao.com/
*
* 修改记录
* 日期              作者                备注
* 2023-01-04        Teternal            first version
* 2024-11-28        Teternal            STC32G migration
********************************************************************************************************************/

#include "board.h"
#include "zf_pwm.h"

#include "driver_interface.h"
#include "charge_control.h"
#include "power_sample.h"

#pragma warning disable = 177
#pragma warning disable = 183

        volatile    charge_state_enum   charge_state            = CHARGE_INIT;  // 充电状态
        volatile    float               charge_power_buff_count = 0;

        volatile    float               charge_power_pidout     = 0;            // 充电 PID 控制输出
static  volatile    float               charge_power_kp         = 0.125;
static  volatile    float               charge_power_ki         = 0.00625;
static  volatile    float               charge_power_kd         = 0;
static  volatile    float               charge_power_err[3];                    // 充电 PID 误差列表

static  volatile    int16               charge_fan_control      = 0;
static  volatile    int16               charge_init_control     = CHARGE_INIT_DISPLAY_MS;

static  volatile    uint16              pwm_period              = 0;
static  volatile    uint16              pwm_phase_diff_max      = CHARGE_MAX_PHASE_DIFF;


static  volatile    uint16              charge_standby_counter = 0;             // 待机计数器
static  volatile    uint16              charge_control_counter = 1;             // 控制执行计数器

#if (DEBUG_PWM_OUTPUT)
        volatile    uint16              charge_debug_pwm_phase_diff = CHARGE_STANDBY_PHASE_DIFF;
#endif


typedef unsigned char volatile far *    pwm_register;

extern const uint32 PWM_CCMR_ADDR[];
extern const uint32 PWM_CCER_ADDR[];
extern const uint32 PWM_CCR_ADDR[];
extern const uint32 PWM_ARR_ADDR[];
extern void pwm_set_gpio (PWMCH_enum pwmch);

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     充电控制 PWM 移相角度设置
// 参数说明     pin             PWM 通道引脚
// 参数说明     phase           相位角度 范围 0 - 10000 对应 0 - 180 度相位偏移
// 返回参数     void
// 使用示例     pwm_phase_diff_set_phase(pin, phase);
// 备注信息     
//-------------------------------------------------------------------------------------------------------------------
void pwm_phase_diff_set_phase (PWMCH_enum pin1, uint16 phase_diff)
{
    uint32 match_temp = (float)pwm_period * (float)phase_diff / 2 / PWM_DUTY_MAX;
    match_temp = (0 == match_temp) ? (1) : (match_temp);

    (*(pwm_register)(PWM_CCR_ADDR[pin1 >> 4]))     = match_temp >> 8;           // 比较值高位
    (*(pwm_register)(PWM_CCR_ADDR[pin1 >> 4] + 1)) = (uint8)match_temp;         // 比较值低位
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     充电控制 PWM 移相初始化
// 参数说明     pin1            A 相 PWM 通道引脚 (可选择范围由 zf_driver_pwm.h 内 pwm_channel_enum 枚举值确定)
// 参数说明     pin2            B 相 PWM 通道引脚 (可选择范围由 zf_driver_pwm.h 内 pwm_channel_enum 枚举值确定)
// 参数说明     freq            频率
// 返回参数     void
// 使用示例     pwm_phase_diff_init(pin1, pin2, freq);
// 备注信息     
//-------------------------------------------------------------------------------------------------------------------
void pwm_phase_diff_init (PWMCH_enum pin1, PWMCH_enum pin2, PWMCH_enum phase_pin, uint32 freq)
{
    uint32  match_temp      = 0;
    uint16  freq_div        = 0;
    uint8   register_temp   = 0;
    
    // P_SW2 0xBA bit7:EAXFR 扩展 RAM 区特殊功能寄存器 (XFR) 访问控制寄存器
    // 当需要访问 XFR 时 必须先将 EAXFR 置 1
    // 才能对 XFR 进行正常的读写
    // 建议上电初始化时直接设置为 1 后续不要再修改
    P_SW2 |= 0x80;
    
    pwm_set_gpio(pin1);
    pwm_set_gpio(pin2);
    pwm_set_gpio(phase_pin);
    
    freq_div = (sys_clk / freq) >> 16;                                          // 分频
    pwm_period = (sys_clk / freq) / (freq_div + 1) - 1;                         // 周期
    match_temp = pwm_period * 0.5;                                              // 占空比

    // PWMA
    {
        // PWMA 从模式设置 串联定时器
        PWMA_SMCR &= 0xF8;
        register_temp = PWMA_SMCR;
        register_temp = register_temp | 0xA0;                                   // 允许通过 ITR2-TRGO 内部触发复位模式
        PWMA_SMCR = register_temp;
        register_temp = register_temp | 0x04;                                   // 允许通过 ITR2-TRGO 内部触发复位模式
        PWMA_SMCR = register_temp;

        // PWMA PWM 输出通道配置
        PWMA_ENO    |= (1 << (pin2 & 0x01)) << ((pin2 >> 4) * 2);               // 使能通道
        PWMA_PS     |= ((pin2 & 0x07) >> 1) << ((pin2 >> 4) * 2);               // 输出脚选择
        (*(pwm_register)(PWM_CCER_ADDR[pin2 >> 5])) |=                          // 配置通道输出使能和极性
            (1 << ((pin2 & 0x01) * 2 + ((pin2 >> 4) & 0x01) * 0x04));

        (*(pwm_register)(PWM_ARR_ADDR[pin2 >> 6]))     = (uint8)(pwm_period>>8);// 周期高位
        (*(pwm_register)(PWM_ARR_ADDR[pin2 >> 6] + 1)) = (uint8)pwm_period;     // 周期低位

        (*(pwm_register)(PWM_CCR_ADDR[pin2 >> 4]))     = match_temp>>8;         // 比较值高位
        (*(pwm_register)(PWM_CCR_ADDR[pin2 >> 4] + 1)) = (uint8)match_temp;     // 比较值低位

        (*(pwm_register)(PWM_CCMR_ADDR[pin2 >> 4]))    |= 0x06 << 4;            // 设置为 PWM 模式 1
        (*(pwm_register)(PWM_CCMR_ADDR[pin2 >> 4]))    |= 1 << 3;               // 开启 PWM 寄存器的预装载功

        // PWMA 使能
        PWMA_PSCRH = (uint8)(freq_div >> 8);                                    // 设置预分频高位
        PWMA_PSCRL = (uint8)freq_div;                                           // 设置预分频低位
        PWMA_BKR &= 0x7F;                                                       // 主输出使能 相当于总开关
        PWMA_CR1 |= 0x01;                                                       // PWM 开始计数
    }
    // PWMB
    {
        // PWMB PWM 输出通道配置
        PWMB_ENO    |= (1 << ((2 * ((pin1 >> 4) - 4))));                        // 使能通道
        PWMB_PS     |= ((pin1 & 0x03) << ((2 * ((pin1 >> 4) - 4))));            // 输出脚选择

        (*(pwm_register)(PWM_CCER_ADDR[pin1 >> 5])) |=                          // 配置通道输出使能和极性
            (uint8)(1 << (((pin1 >> 4) & 0x01) * 4));

        (*(pwm_register)(PWM_ARR_ADDR[pin1 >> 6]))     = (uint8)(pwm_period>>8);// 周期高位
        (*(pwm_register)(PWM_ARR_ADDR[pin1 >> 6] + 1)) = (uint8)pwm_period;     // 周期低位

        (*(pwm_register)(PWM_CCR_ADDR[pin1 >> 4]))     = match_temp>>8;         // 比较值高位
        (*(pwm_register)(PWM_CCR_ADDR[pin1 >> 4] + 1)) = (uint8)match_temp;     // 比较值低位

        (*(pwm_register)(PWM_CCMR_ADDR[pin1 >> 4]))    |= 0x06 << 4;            // 设置为 PWM 模式 1
        (*(pwm_register)(PWM_CCMR_ADDR[pin1 >> 4]))    |= 1 << 3;               // 开启 PWM 寄存器的预装载功

        // PWMB 串联同步通道 TRGO 信号配置
        // 只有第二组 PWM(PWMB)的 TRGO 可用于第一组 PWM(PWMA)的 内部触发 ITR2 
        PWMB_ENO |= (1 << ((2 * ((phase_pin >> 4) - 4))));                      // 使能通道
        PWMB_PS  |= ((phase_pin & 0x03) << ((2 * ((phase_pin >> 4) - 4))));     // 输出脚选择

        (*(pwm_register)(PWM_CCER_ADDR[phase_pin >> 5])) |=                     // 配置通道输出使能和极性
            (uint8)(3 << (((phase_pin >> 4) & 0x01) * 4));

        (*(pwm_register)(PWM_ARR_ADDR[phase_pin >> 6]))     = (uint8)(pwm_period>>8);   // 周期高位
        (*(pwm_register)(PWM_ARR_ADDR[phase_pin >> 6] + 1)) = (uint8)pwm_period;        // 周期低位

        (*(pwm_register)(PWM_CCR_ADDR[phase_pin >> 4]))     = 0;                // 比较值高位
        (*(pwm_register)(PWM_CCR_ADDR[phase_pin >> 4] + 1)) = 1;                // 比较值低位

        (*(pwm_register)(PWM_CCMR_ADDR[phase_pin >> 4]))    |= 0x07 << 4;       // 设置为 PWM 模式 1
        (*(pwm_register)(PWM_CCMR_ADDR[phase_pin >> 4]))    |= 1 << 3;          // 开启 PWM 寄存器的预装载功

        register_temp = PWMB_CR2;
        register_temp = (register_temp & 0x8F) | (phase_pin & 0xF0);            // 选择对应 OCxREF 信号被用于作为触发输出 (TRGO)
        PWMB_CR2 = register_temp;

        // PWMB 使能
        PWMB_PSCRH = (uint8)(freq_div >> 8);                                    // 设置预分频高位
        PWMB_PSCRL = (uint8)freq_div;                                           // 设置预分频低位
        PWMB_BKR &= 0x7F;                                                       // 主输出使能 相当于总开关
        PWMB_CR1 |= 0x01;                                                       // PWM 开始计数
    }
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     充电控制 PWM 控制
// 参数说明     phase_diff      相位偏移 0-1000 对应 0-180 度相位偏移
// 返回参数     void
// 使用示例     charge_pwm_set_phase_diff((uint16)charge_power_pidout);
// 备注信息     使用定时器从模式复位模式 实现串级定时器移相 PWM 输出功能 所以这里调整复位用的通道来调整移相角
//-------------------------------------------------------------------------------------------------------------------
void charge_pwm_set_phase_diff (uint16 phase_diff)
{
    phase_diff = (CHARGE_MAX_PHASE_DIFF <= phase_diff) ? (CHARGE_MAX_PHASE_DIFF) : (phase_diff);
    pwm_phase_diff_set_phase(CHARGE_PWM_PHASE_PIN, phase_diff * 10);
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     充电控制 使能接口
// 参数说明     enable      使能
// 返回参数     void
// 使用示例     charge_congrol_enable(ZF_ENABLE);
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void charge_congrol_enable (uint8 enable)
{
    if(enable)
    {
        gpio_set_level(CHARGE_LED_PIN, (CHARGE_LED_EN));
        gpio_set_level(CHARGE_FAN_PIN, (CHARGE_FAN_EN));
        gpio_set_level(CHARGE_PWMEN_PIN, (CHARGE_PWMEN_EN));

        charge_power_buff_count = 0;                                            // 清空阶梯控制计数

        charge_power_err[2] = 0;
        charge_power_err[1] = 0;
        charge_power_err[0] = 0;
        charge_power_pidout = CHARGE_STANDBY_PHASE_DIFF;

        charge_pwm_set_phase_diff(CHARGE_STANDBY_PHASE_DIFF);                   // 开启时固定从待机移相角开启

        PWMB_BKR |= 0x80;
        PWMA_BKR |= 0x80;
    }
    else
    {
        PWMB_BKR &= 0x7F;
        PWMA_BKR &= 0x7F;

        gpio_set_level(CHARGE_PWMEN_PIN, (!CHARGE_PWMEN_EN));
        gpio_set_level(CHARGE_LED_PIN, (!CHARGE_LED_EN));
    }
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     充电控制 定时器周期函数
// 参数说明     void
// 返回参数     void
// 使用示例     charge_control_handler();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void charge_control_handler (void)
{
    float pidout_temp = 0.0;

#if(CHARGE_TEST_PLUS_ENABLE)
    gpio_set_level(CHARGE_TEST_PLUS_PIN, 1);
#endif
    if(CHARGE_INIT == charge_state)
    {
        if(CHARGE_INIT_DISPLAY_MS / 2 > charge_init_control)
        {
            power_sample_check_battery();
        }
    }
    else
    {
        power_sample_handler();
    }

    if(CHARGE_DISPLAY_POWER_PERIOD_MS == charge_control_counter ++)             // 功率显示刷新判断
    {
#if (DEBUG_CURRENT_DISPLAY)
        charge_display_power = bus_current * 10.0;                              // 显示电流更新
#else
        charge_display_power = charge_power;                                    // 显示功率更新
#endif
        charge_control_counter = 1;                                             // 重新计数
    }

    // 根据目标值调节PWM占空比的值
    switch(charge_state)
    {
        default:
        case CHARGE_ERR:
        case CHARGE_LOW_POWER:
        case CHARGE_CLOSE:
        {
            charge_power_pidout = 0;
            charge_congrol_enable(ZF_DISABLE);
        }break;
        case CHARGE_INIT:
        {
            charge_power_pidout = 0;
            charge_congrol_enable(ZF_DISABLE);
            if(0 < charge_init_control)
            {
                charge_init_control --;
            }
            else
            {
                charge_state = CHARGE_CLOSE;
            }
        }break;
        case CHARGE_OPEN:
        {
            charge_fan_control = CHARGE_FAN_CLOSE_DELAY_MS;                     // 更新风扇的关闭延迟 防止运行时停止
            charge_power_err[2] = charge_power_err[1];
            charge_power_err[1] = charge_power_err[0];

            // 这里采用阶梯功率控制 主要是因为电流采样信号有充电效应
            // 由于电流采样输出一开始会低于真实功率 这样会导致失真超调
            // 因此分成三个阶梯控制
            // 0.0s-1.0s 线性调控 50%-100% 目标功率
            // 1.0s 往后为 100% 目标功率
            if(1000 > charge_power_buff_count)
            {
                charge_power_err[0] = (float)CHARGE_POWER_TARGET * (0.5 + charge_power_buff_count / 2000.0) - charge_power;
                charge_power_buff_count ++;
            }
            else
            {
                charge_power_err[0] = (float)CHARGE_POWER_TARGET - charge_power;
            }

            // 功率控制部分允许低于目标功率 但严格限制超过目标功率
            // 因此低于目标功率超过 10W 时正常调控
            // 在 -5W - 10W 范围时削弱调控
            // 在超过目标功率 5W 时严格调控下调移相
            // 这样可能会导致一定震荡 但是可以避免滞后造成的长时间超调
            if(10 < charge_power_err[0])
            {
                pidout_temp = ((charge_power_err[0] - charge_power_err[1]) * charge_power_kp +
                               charge_power_err[0] * charge_power_ki +
                               (charge_power_err[0] - 2 * charge_power_err[1] + charge_power_err[2]) * charge_power_kd) * 6;
            }
            else if(-5 > charge_power_err[0])
            {
                pidout_temp = ((charge_power_err[0] - charge_power_err[1]) * charge_power_kp +
                               charge_power_err[0] * charge_power_ki +
                               (charge_power_err[0] - 2 * charge_power_err[1] + charge_power_err[2]) * charge_power_kd) * 24;
            }
            else
            {
                pidout_temp = ((charge_power_err[0] - charge_power_err[1]) * charge_power_kp +
                               charge_power_err[0] * charge_power_ki +
                               (charge_power_err[0] - 2 * charge_power_err[1] + charge_power_err[2]) * charge_power_kd);
            }
            charge_power_pidout += pidout_temp;
            charge_power_pidout = func_limit_ab(charge_power_pidout, 50, pwm_phase_diff_max);
#if(DEBUG_PWM_OUTPUT)
            charge_pwm_set_phase_diff(func_limit_ab(charge_debug_pwm_phase_diff, 50, CHARGE_MAX_PHASE_DIFF));
#else
            if(CHARGE_STANDBY_POWER_LOW_LIMIT > charge_power)
            {
                charge_standby_counter ++;
                if(CHARGE_STANDBY_TIME_MS < charge_standby_counter)
                {
                    charge_standby_counter /= 2;
                    charge_state = CHARGE_STANDBY;
                }
            }
            charge_power_pidout = func_limit_ab(charge_power_pidout, 50, CHARGE_MAX_PHASE_DIFF);
            charge_pwm_set_phase_diff((uint16)charge_power_pidout);
#endif
        }break;
        case CHARGE_STANDBY:
        {
            charge_fan_control = CHARGE_FAN_CLOSE_DELAY_MS;
            if(CHARGE_STANDBY_POWER_LOW_LIMIT <= charge_power)
            {
                charge_standby_counter --;
                if(0 == charge_standby_counter)
                {
                    charge_state = CHARGE_OPEN;
                }
            }
            charge_pwm_set_phase_diff((uint16)CHARGE_STANDBY_PHASE_DIFF);
        }break;
    }

    if(0 < charge_fan_control)
    {
        charge_fan_control --;
    }
    else
    {
        gpio_set_level(CHARGE_FAN_PIN, (!CHARGE_FAN_EN));
    }

#if(CHARGE_TEST_PLUS_ENABLE)
    gpio_set_level(CHARGE_TEST_PLUS_PIN, 0);
#endif
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     充电控制 初始化 主要是采样初始化
// 参数说明     void
// 返回参数     void
// 使用示例     charge_init();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void charge_init (void)
{

    // ------------------- 充电控制 散热与指示灯 部分初始化 -------------------
    gpio_mode(CHARGE_FAN_PIN, GPO_PP);
    gpio_mode(CHARGE_LED_PIN, GPO_PP);
    gpio_set_level(CHARGE_FAN_PIN, (!CHARGE_FAN_EN));
    gpio_set_level(CHARGE_LED_PIN, (!CHARGE_LED_EN));
    // ------------------- 充电控制 散热与指示灯 部分初始化 -------------------

    // ------------------- 充电控制 PWM 部分初始化 -------------------
    gpio_mode(CHARGE_PWMEN_PIN, GPO_PP);
    gpio_set_level(CHARGE_PWMEN_PIN, (!CHARGE_PWMEN_EN));

    pwm_phase_diff_init(CHARGE_PWMA_PIN, CHARGE_PWMB_PIN, CHARGE_PWM_PHASE_PIN, 150 * 1000);
    // ------------------- 充电控制 PWM 部分初始化 -------------------

    // ------------------- 充电控制 测试 部分初始化 -------------------
#if(CHARGE_TEST_PLUS_ENABLE)
    gpio_mode(CHARGE_TEST_PLUS_PIN, GPO_PP);
    gpio_set_level(CHARGE_TEST_PLUS_PIN, 0);
#endif
    // ------------------- 充电控制 测试 部分初始化 -------------------
}
